%> @file run_me.m
%> @brief Running test script to run MPC and LQG controllers for the Adaptive Optics plant. This is a basic testbed for the algorithms testing and studying.
%>
%> @author Mikhail Konnik
%> @date   8 January 2012
%>

clc
clear all
close all
addpath('/home/think/matlab/aorta-ffcvsmpc/simulator/tools/')
load('colourmap_for_hessian','mycmap') 

cc.N_sim=4000;  %% for saved noise - only 1000 points are used.

cc.T_f = 0.001;  %%% sampling period [seconds]

global cc;

cc.matrix.Hessian.thresh = 10^(-2); %% threshold for the Hessian matrix
cc.matrix.F.thresh = 10^(-2); %% threshold for the F matrix
cc.matrix.inv_Hessian.thresh = 10^(-6); %% threshold for the inverted Hessian matrix

% cc.matrix.Hessian.thresh = 10^(-10); %% threshold for the Hessian matrix
% cc.matrix.F.thresh = 10^(-10); %% threshold for the F matrix
% cc.matrix.inv_Hessian.thresh = 10^(-10); %% threshold for the inverted Hessian matrix



cc.plant.coupling_degree = 0.20;


% 
%  cc.plant.size = '4c';  %%% 2x2 actuators
% %  cc.plant.size = '4cdiag';  %%% 2x2 actuators
%  cc.plant.size_num = [4,4]; %%% how many actuators [rows,cols] does the plant have?
% 
% %  
%   cc.plant.size = '9c'; %%% 3x3 actuators, Letter C means that the plant is coupled.
% %   cc.plant.size = '9cdiag';  %%% 3x3 actuators
%   cc.plant.size_num = [9,9]; %%% how many actuators [rows,cols] does the plant have?
% % % % 
  cc.plant.size = '16c'; %%% 4x4 actuators, Letter C means that the plant is coupled.
%   cc.plant.size = '16cdiag'; %%% 4x4 actuators, Letter C means that the plant is coupled.
  cc.plant.size_num = [16,16]; %%% how many actuators [rows,cols] does the plant have?
% % % 
% % % % 
% cc.plant.size = '25c'; %%% 5x5 actuators, Letter C means that the plant is coupled.
% % cc.plant.size = '25cdiag'; %%% 5x5 actuators, Letter C means that the plant is coupled.
% cc.plant.size_num = [25,25]; %%% how many actuators [rows,cols] does the plant have?
% % % % 
% % % % 
% cc.plant.size = '36c'; %%% 6x6 actuators, Letter C means that the plant is coupled.
% %   cc.plant.size = '36cdiag'; %%% 6x6 actuators, Letter C means that the plant is coupled.
% cc.plant.size_num = [36,36]; %%% how many actuators [rows,cols] does the plant have?
% % % 
% % % 
% %   cc.plant.size = '49'; %%% 7x7 actuators, Letter C means that the plant is coupled.
%   cc.plant.size = '49c'; %%% 7x7 actuators, Letter C means that the plant is coupled.
% %   cc.plant.size = '49cdiag'; %%% 7x7 actuators, Letter C means that the plant is coupled.
%   cc.plant.size_num = [49,49]; %%% how many actuators [rows,cols] does the plant have?
% % 
% cc.plant.size = '64c'; %%% 8x8 actuators, Letter C means that the plant is coupled.
% % cc.plant.size = '64cdiag'; %%% 8x8 actuators, Letter C means that the plant is coupled.
% cc.plant.size_num = [64,64]; %%% how many actuators [rows,cols] does the plant have?
% % 
% cc.plant.size = '81c'; %%% 9x9 actuators, Letter C means that the plant is coupled.
% % cc.plant.size = '81cdiag'; %%% 9x9 actuators, Letter C means that the plant is coupled.
% cc.plant.size_num = [81,81]; %%% how many actuators [rows,cols] does the plant have?
% 
%   cc.plant.size = '100c'; %%% 10x10 actuators, Letter C means that the plant is coupled.
% %   cc.plant.size = '100cdiag'; %%% 10x10 actuators, Letter C means that the plant is coupled.
%   cc.plant.size_num = [100,100]; %%% how many actuators [rows,cols] does the plant have?




cc.flag.plant_equations = 'pre_coded';  %%% valid are: 'rand_generated', 'pre_coded'
cc.flag.sparsemode = 1; %%% whenever sparse methods are used or not.
cc.flag.savednoise = 1; %% use the same noise, which was saved into CSV file, for study or comparisons


%%%%% Unconstrained case
%  cc.constraints.u_max =  60000*ones(cc.plant.size_num(1)*cc.plant.size_num(2));   %%%% <---- MAXIMUM Constraints on the control amplitude
%  cc.constraints.u_min = -60000*ones(cc.plant.size_num(1)*cc.plant.size_num(2));  %%%% <---- MINIMUM Constraints on the control amplitude.
%  cc.constraints.u_max =  60000*ones(cc.plant.size_num(1),1);   %%%% <---- MAXIMUM Constraints on the control amplitude
%  cc.constraints.u_min= -60000*ones(cc.plant.size_num(1),1);  %%%% <---- MINIMUM Constraints on the control amplitude.


%  %%%% Constrained case, Realistic case, constraints are violated rarely.
cc.constraints.u_max =  230*ones(cc.plant.size_num(1),1);   %%%% <---- MAXIMUM Constraints on the control amplitude.
cc.constraints.u_min= -230*ones(cc.plant.size_num(1),1);  %%%% <---- MINIMUM Constraints on the control amplitude.
% 

%  %  %%%%% Constrained case, Worst case (constraints are violated ALL THE TIME)
%  cc.constraints.u_max =  100*ones(cc.plant.size_num(1)*cc.plant.size_num(2));   %%%% <---- MAXIMUM Constraints on the control amplitude.
%  cc.constraints.u_min= -100*ones(cc.plant.size_num(1)*cc.plant.size_num(2));  %%%% <---- MINIMUM Constraints on the control amplitude.
% cc.constraints.u_max =  100*ones(cc.plant.size_num(1),1);   %%%% <---- MAXIMUM Constraints on the control amplitude
% cc.constraints.u_min = -100*ones(cc.plant.size_num(1),1);  %%%% <---- MINIMUM Constraints on the control amplitude.




%%%%%% This is for saturator to show the impact of the uncostained controllers
%  cc.constraints.u_max_sat = 100;
%  cc.constraints.u_min_sat = -100;

%%%%%%% #### START: Obtain the Transfer matrix of the Plant and Disturbance #####%%%
[cc_plant_nomin,cc_plant_denom,cc_atm_nomin,cc_atm_denom] = cc_MIMO_plant_transfermatrix_formulation(cc.flag.plant_equations,cc.plant.size,cc.plant.coupling_degree);
%%%%%%% #### ##END: Obtain the Transfer matrix of the Plant and Disturbance #####%%%

%  break

%  %%%%%%% #### START: Transfer matrix TO State space #####%%%
global Ap Bp Cp Dp;
[Ap,Bp,Cp,Dp] = cc_cell_tf2ss(cc_plant_nomin,cc_plant_denom,cc.T_f);

global Ad Bd Cd Dd;
[Ad,Bd,Cd,Dd] = cc_cell_tf2ss(cc_atm_nomin,cc_atm_denom,cc.T_f);
%  %%%%%%% ###### END: Transfer matrix TO State space #####%%%

%  break

%%%%%%% START:  This is matrix augmentation to account the disturbance %%%%%%%%%%
global A_e B_e C_e G;
[A_e,B_e,C_e,G] = cc_statespace_augmentation_for_disturbance_rejection(Ap,Bp,Cp,Dp,Ad,Bd,Cd,Dd,cc.flag.sparsemode);
%%%%%%% END:  This is matrix augmentation to account the disturbance %%%%%%%%%%


%  break

%%%%%%%% START: Preparing the matrices for the simulation and results storage %%%%%%%
[m1,c1]=size(Cp);
[n1,n_in]=size(Bp);

[dm1,dc1]=size(Cd);
[dn1,dn_in]=size(Bd);

xm=zeros(c1,1);
xd=zeros(dc1,1);

u=zeros(n_in,1); % u(k-1) =0
y=zeros(m1,1);


[ny,n]=size(C_e);
[n,nu]=size(B_e);
%%%%%%%% ### END: Preparing the matrices for the simulation and results storage %%%%%%%

if (cc.flag.savednoise == 1)
	v_saved = csvread(strcat('process_noise_randn_plant_',num2str(cc.plant.size_num(1)),'c_points_',num2str(cc.N_sim),'.csv'));
end %% if (cc.flag.savednoise == 1)



% 
% iter1 = [];
%%%%% ## START:  The simulation of the controller and the controller input to the plant %%%%
for kk=1:cc.N_sim

%%%%%%%%%%%%%%%%%%% MPC/RHC Controllers %%%%%%%%%%%%%%%%%%%%%%%%%%%
tic
             u = study_mpc_constr_DeDonaoutdisturbreject_ccsparse_optimized(y);  %% QA passed OK. CURRENT SPARSE
t=toc;


if (cc.flag.savednoise == 1)
	    v = v_saved(:,kk);
	else
            v = 26*randn([dn_in,1]);
end %%% if (cc.flag.savednoise == 1)

            measurement_noise = 0.00001*randn([n_in,1]);

           
           %plant simulation
           xm = Ap*xm + Bp*u;          % calculate xm(k+1)
           xd = Ad*xd + Bd*v;          % calculate xm(k+1)
           y = [Cp,Cd]*[xm;xd] + measurement_noise;                  %calculate y(k+1)
           yd = Cd*xd +measurement_noise;
           %plant simulation


            u1(1:nu,kk)=u;                    %keep u
            y1(1:ny,kk)=y;                    %keep y
            y1d(1:ny,kk)=yd;                    %keep y
            time_of_controller_execution(kk) = t;


% fprintf('Solution has converged within %d iterations on Step %d. \n',cc.iter,kk);

%  %%%%%%% The string below is to generate and save the noise into a file - DO NOT ERASE IT!!!!!
%  		v1(:,kk) = v;
%  %
end  %%%%% ##### END:  The simulation of the controller and the controller input to the plant %%%%



%  %%%%%%% The string below is to generate and save the noise into a file - DO NOT ERASE IT!!!!!
%  csvwrite(strcat('process_noise_randn_plant_',cc.plant.size,'_points_',num2str(cc.N_sim),'.csv'),v1);
%  fprintf('The output disturbance for the Plant %s has been written down to a file. \n', cc.plant.size);
%  %%%%%%% The string up is to generate and save the noise into a file - DO NOT ERASE IT!!!!!

% cc.plant.size = strcat(cc.plant.size,'decoupled'); 


  k=0:(cc.N_sim-1);
  figure(10)
  subplot(311)
  plot(k,y1)
  xlabel('Sampling Instant')
  ylabel('Plant output')
  subplot(312)
  plot(k,y1d)
  xlabel('Disturbance')
  ylabel('Disturbance')
  subplot(313)
  plot(k,u1)
  xlabel('Sampling Instant')
  ylabel('Control Signal')

  fprintf('\n\nOutput Disturbance std %1.6f was rejected down to std %1.6f \n',std(y1d(1,:)),std(y1(1,:)) );
  fprintf('Median time to execute the MPC controller: %1.6f, i.e., speed is %4.1f Hz \n', median(time_of_controller_execution(3:end)), 1/median(time_of_controller_execution(3:end)));
  fprintf('Mean time to execute the MPC controller  : %1.6f, i.e., speed is %4.1f Hz \n', mean(time_of_controller_execution(3:end)), 1/mean(time_of_controller_execution(3:end)));
